"""
Time scanning and interval extraction utilities.

Given a boolean predicate "is_occluded(t) -> bool", this module finds the union
of time intervals where the predicate is True. Edges are refined by bisection.
"""

from __future__ import annotations
from typing import Callable, List, Tuple
import numpy as np

def find_boolean_intervals(
    t0: float,
    t1: float,
    predicate: Callable[[float], bool],
    dt: float = 1,
    refine_iters: int = 4
) -> list[tuple[float, float]]:
    """在时间区间[t0,t1]上，对一个布尔判定函数进行等步长扫描，找出所有 True 的时间片段，

    Args:
        t0 (float): 起始时间
        t1 (float): 中止时间
        predicate (Callable[[float], bool]): bool函数，接受时间t给出true/false
        dt (float, optional): 求解时间步长. Defaults to 0.01.
        refine_iters (int, optional): 时间边界二分法精细迭代数. Defaults to 40.

    Returns:
        list[tuple[float, float]]: 所有为true的interval
    """
    if t1 <= t0:
        return []

    times = []
    flags = []
    t = t0
    while t <= t1 + 1e-12:
        times.append(t)
        flags.append(bool(predicate(t)))
        t += dt

    if not times:
        return []

    intervals = []
    inside = flags[0]
    start = times[0] if inside else None

    # Helper: bisection on time for boolean switch
    def bisect_edge(a: float, b: float, target_state: bool) -> float:
        lo, hi = a, b
        for _ in range(refine_iters):
            mid = 0.5 * (lo + hi)
            if bool(predicate(mid)) == target_state:
                hi = mid
            else:
                lo = mid
        return hi if target_state else lo

    for i in range(1, len(times)):
        if inside and not flags[i]:
            # Leaving occlusion between times[i-1] and times[i]
            edge = bisect_edge(times[i-1], times[i], target_state=False)
            intervals.append((start, edge))
            inside = False
            start = None
        elif (not inside) and flags[i]:
            # Entering occlusion
            start = bisect_edge(times[i-1], times[i], target_state=True)
            inside = True

    if inside and start is not None:
        intervals.append((start, times[-1]))

    # Merge touching/overlapping intervals (robustness)
    merged = []
    for a, b in intervals:
        if not merged or a > merged[-1][1] + 1e-9:
            merged.append([a, b])
        else:
            merged[-1][1] = max(merged[-1][1], b)
    return [(a, b) for a, b in merged]


def union_length(intervals: list[tuple[float, float]]) -> float:
    """Total length (seconds) of a set of [a, b] intervals (assumed a<=b)."""
    if not intervals:
        return 0.0
    ints = sorted(intervals)
    total = 0.0
    a0, b0 = ints[0]
    for a, b in ints[1:]:
        if a <= b0:
            b0 = max(b0, b)
        else:
            total += b0 - a0
            a0, b0 = a, b
    total += b0 - a0
    return float(total)

"""
Point-LoS occlusion (analytic, no time scanning).

This module computes the exact time intervals when a spherical cloud (center C(t), radius r)
blocks the line-of-sight (segment) from a moving missile M(t) to a fixed target point T.

Key idea:
    distance^2( C(t), line[T, M(t)] ) = || (C(t)-T) × (M(t)-T) ||^2 / ||M(t)-T||^2
Both C(t) and M(t) are linear in t after detonation, therefore the numerator is a quadratic
polynomial vector in t, and its squared norm is a quartic polynomial. The denominator is
quadratic. The occlusion condition "distance ≤ r" becomes a quartic inequality.

Additionally, for segment (not infinite line), the perpendicular foot must lie between T and M(t).
This is equivalent to: 0 ≤ s*(t) ≤ 1, where
    s*(t) = 1 - ( (C(t)-T)·(M(t)-T) ) / ||M(t)-T||^2
which reduces to two quadratic inequalities:
    (C(t)-T)·(M(t)-T) ≥ 0       and
    (C(t)-T)·(M(t)-T) ≤ ||M(t)-T||^2

We compute all real roots of these polynomials, sort them along [t0, t1], and test midpoints
to extract the union of intervals where all three constraints hold.

This gives exact cover intervals WITHOUT any time discretization or boolean bisection.

Usage:
    from src.core.occlusion.point_analytic import intervals_point_los_analytic

    intervals, total = intervals_point_los_analytic(
        M0, Vm_dir, Vm_speed,
        T_point,
        C_det, sink, t_det,
        r,
        t0=t_det,
        t1=min(t_det+life, t_hit+margin)
    )
"""



def _poly_cross_linear_linear_sqnorm(Wc, F, U0, D) -> np.ndarray:
    """
    Build coefficients of || (Wc + F t) × (U0 + D t) ||^2, as a quartic polynomial in t.

    Returns
    -------
    coeffs : np.ndarray shape (5,)
        [k4, k3, k2, k1, k0] so that value(t) = k4 t^4 + k3 t^3 + k2 t^2 + k1 t + k0
    """
    A0 = np.cross(Wc, U0)          # constant term
    A1 = np.cross(Wc, D) + np.cross(F, U0)   # linear term
    A2 = np.cross(F, D)            # quadratic term

    # ||A0 + A1 t + A2 t^2||^2 = sum_j (A0j + A1j t + A2j t^2)^2
    # Expand per component and accumulate coefficients
    k0 = float(np.dot(A0, A0))
    k1 = float(2.0 * np.dot(A0, A1))
    k2 = float(np.dot(A1, A1) + 2.0 * np.dot(A0, A2))
    k3 = float(2.0 * np.dot(A1, A2))
    k4 = float(np.dot(A2, A2))
    return np.array([k4, k3, k2, k1, k0], dtype=float)


def _poly_u2(U0, D) -> np.ndarray:
    """
    Build coefficients of ||U0 + D t||^2 = c2 t^2 + c1 t + c0 (quadratic).

    Returns [c2, c1, c0].
    """
    c2 = float(np.dot(D, D))
    c1 = float(2.0 * np.dot(U0, D))
    c0 = float(np.dot(U0, U0))
    return np.array([c2, c1, c0], dtype=float)


def _poly_dot_quadratic(Wc, F, U0, D) -> np.ndarray:
    """
    Build coefficients of (Wc + F t)·(U0 + D t) = d2 t^2 + d1 t + d0 (quadratic).
    Returns [d2, d1, d0].
    """
    d2 = float(np.dot(F, D))
    d1 = float(np.dot(Wc, D) + np.dot(F, U0))
    d0 = float(np.dot(Wc, U0))
    return np.array([d2, d1, d0], dtype=float)


def _poly_subtract_scaled_quadratic(quartic, quad, scale) -> np.ndarray:
    """
    Compute quartic(t) - scale * quad(t), where quad is degree-2 and scale is scalar.
    Returns a quartic coefficients array of shape (5,). Assumes quartic is [k4,k3,k2,k1,k0]
    and quad is [c2,c1,c0].
    """
    k4, k3, k2, k1, k0 = quartic
    c2, c1, c0 = quad
    return np.array([
        k4,              # t^4
        k3,              # t^3
        k2 - scale*c2,   # t^2
        k1 - scale*c1,   # t^1
        k0 - scale*c0    # t^0
    ], dtype=float)


def _real_roots_in_interval(coeffs: np.ndarray, a: float, b: float, eps: float = 1e-12) -> list:
    """
    Return sorted real roots of a polynomial within [a, b], inclusive, using numpy.roots.
    coeffs should be in descending powers (e.g., [k4,k3,k2,k1,k0]).
    """
    # Remove leading zeros to avoid numpy warnings
    c = np.array(coeffs, dtype=float)
    # Trim leading zeros
    idx = 0
    while idx < len(c)-1 and abs(c[idx]) < eps:
        idx += 1
    c = c[idx:]
    if len(c) <= 1:
        return []
    roots = np.roots(c)
    real = []
    for r in roots:
        if abs(r.imag) < 1e-10:
            x = float(r.real)
            if a - 1e-12 <= x <= b + 1e-12:
                real.append(x)
    real.sort()
    return real


def intervals_point_los_analytic(
    M0: np.ndarray,
    Vm_dir: np.ndarray,
    Vm_speed: float,
    T: np.ndarray,
    C_det: np.ndarray,
    sink: float,
    t_det: float,
    r: float,
    t0: float,
    t1: float
) -> tuple[list[tuple[float,float]], float]:
    """
    Compute exact occlusion intervals for the representative point LoS model.

    Parameters
    ----------
    M0 : (3,) array
        Missile initial position at t=0.
    Vm_dir : (3,) array (unit)
        Missile direction (towards aim point). M(t) = M0 + Vm_speed * Vm_dir * t
    Vm_speed : float
        Missile speed (m/s).
    T : (3,) array
        Target representative point (e.g., cylinder center).
    C_det : (3,) array
        Cloud center at detonation time t_det.
    sink : float
        Cloud vertical sink speed (m/s), active for t >= t_det.
    t_det : float
        Detonation time (s).
    r : float
        Cloud radius (m).
    t0, t1 : float
        Time window to search (typically [t_det, min(t_det+life, t_hit+margin)]).

    Returns
    -------
    intervals : list[(a,b)]
        Disjoint intervals where occlusion holds.
    total : float
        Total covered time (sum of lengths of intervals).
    """
    if t1 <= t0:
        return [], 0.0
    # Precompute linear models:
    # U(t) = M(t) - T = U0 + D t
    D = Vm_speed * np.asarray(Vm_dir, dtype=float)
    U0 = np.asarray(M0, dtype=float) - np.asarray(T, dtype=float)

    # C(t) = C_det + [0,0,-sink]*(t - t_det)  =>  W(t) = C(t) - T = Wc + F t
    F = np.array([0.0, 0.0, -float(sink)], dtype=float)
    Wc = (np.asarray(C_det, dtype=float) - np.asarray(T, dtype=float)) - F * float(t_det)

    # Build polynomials
    # g(t) := ||W×U||^2 - r^2 ||U||^2 ≤ 0  (quartic inequality)
    quart = _poly_cross_linear_linear_sqnorm(Wc, F, U0, D)
    u2 = _poly_u2(U0, D)
    g = _poly_subtract_scaled_quadratic(quart, u2, r*r)  # quartic

    # h1(t) := (W·U) ≥ 0
    h1 = _poly_dot_quadratic(Wc, F, U0, D)  # quadratic coeffs [d2,d1,d0]
    # h2(t) := (W·U) - ||U||^2 ≤ 0
    h2 = np.array([h1[0] - u2[0], h1[1] - u2[1], h1[2] - u2[2]], dtype=float)

    # Collect critical times
    crit = [t0, t1]
    crit += _real_roots_in_interval(g, t0, t1)
    crit += _real_roots_in_interval(h1, t0, t1)
    crit += _real_roots_in_interval(h2, t0, t1)
    crit = sorted(set(float(x) for x in crit))

    def eval_poly(c, t: float) -> float:
        # c descending powers
        tpow = 1.0
        val = 0.0
        # Horner scheme
        val = 0.0
        for coeff in c:
            val = val * t + coeff
        return val

    def cond_at(t: float) -> bool:
        # All constraints
        gt = eval_poly(g, t) <= 1e-12  # distance constraint
        # For h1/h2 we need values; they are quadratics: [a,b,c] for a*t^2 + b*t + c
        h1v = h1[0]*t*t + h1[1]*t + h1[2]
        h2v = h2[0]*t*t + h2[1]*t + h2[2]
        return gt and (h1v >= -1e-12) and (h2v <= 1e-12)

    # Sweep intervals between consecutive critical points
    intervals = []
    for i in range(len(crit)-1):
        a = crit[i]; b = crit[i+1]
        if b <= a: continue
        m = 0.5*(a+b)
        if cond_at(m):
            intervals.append((a, b))

    # Merge numerically touching intervals
    merged = []
    for a,b in intervals:
        if not merged or a > merged[-1][1] + 1e-10:
            merged.append([a,b])
        else:
            merged[-1][1] = max(merged[-1][1], b)

    res = [(float(a), float(b)) for a,b in merged]
    total = float(sum(b-a for a,b in res))
    return res, total